// Copyright (C) 2012 Yury Bychenkov (http://bychenkov.com).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//  - Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//  - Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
//  - Neither the name of Yury Bychenkov nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

template <typename ElementTraits>
uint Heap<ElementTraits>::getCount() const {
	return count;
}

template <typename ElementTraits>
const uint8* Heap<ElementTraits>::get(uint i) const {
	return data + heap[i];
}

template <typename ElementTraits>
const uint8* Heap<ElementTraits>::peek() const {
	return data + heap[0];
}

template <typename ElementTraits>
void Heap<ElementTraits>::remove() {
	if (count > 1) {
		const uint size = ElementTraits::sizeOf(data + heap[0]);
		if (size + heap[0] < dataSize) {
			memset(data + heap[0], 0xFF, size);
		}

		heap[0] = heap[--count];
		fixDown();
	} else {
		count = 0;
		dataSize = 0;
	}
}


template <typename ElementTraits>
Heap<ElementTraits>::Heap(uint8* memory, uint memorySize, uint countThreshold_):
	countThreshold(countThreshold_),

	heap((uint32*) memory),
	count(0),

	data((uint8*) (heap + countThreshold)),
	dataCapacity(memorySize - countThreshold * sizeof(uint32)),
	dataSize(0),
	garbageSize(0)
{	
}

template <typename ElementTraits>
void Heap<ElementTraits>::fixUp() {
	uint k = count - 1;
	const uint8* element = data + heap[k];
	while (k > 0) {
		uint r = (k - 1) / 2;
		if (ElementTraits::compare(data + heap[r], element) >= 0) {
			break;
		}
		swap(heap[k], heap[r]);
		k = r;
	}
}

template <typename ElementTraits>
void Heap<ElementTraits>::fixDown() {
	uint k = 0;
	const uint8* element = data + heap[k];
	while (true) {
		const uint r = k * 2 + 1;
		if (r >= count) {
			break;
		}

		if (ElementTraits::compare(data + heap[r], element) > 0) {
			if (r + 1 < count && ElementTraits::compare(data + heap[r + 1], data + heap[r]) > 0) {
				swap(heap[r + 1], heap[k]);
				k = r + 1;
			} else {
				swap(heap[r], heap[k]);
				k = r;
			}
		} else if (r + 1 < count && ElementTraits::compare(data + heap[r + 1], element) > 0) {
			swap(heap[r + 1], heap[k]);
			k = r + 1;
		} else {
			break;
		}
	}
}

template <typename ElementTraits>
void Heap<ElementTraits>::deflate() {	
	uint8* dst = data;
	const uint8* src = data;
	const uint8* end = data + dataSize;

	while (src < end) {
		if (*src == 0xFF) {
			src++;
			continue;
		}

		const uint8* p = src;
		do {
			src += ElementTraits::sizeOf(src);
		} while (src < end && *src != 0xFF);

		memmove(dst, p, src - p);
		dst += src - p;
   	}
 	garbageSize = 0;

	dataSize = 0;
	for (uint i = 0; i < count; i++) {
		uint k = i;

		heap[k] = dataSize;
		dataSize += ElementTraits::sizeOf(data + dataSize);
		while (k > 0) {
			uint r = (k - 1) / 2;
			if (ElementTraits::compare(data + heap[r], data + heap[k]) >= 0) {
				break;
			}
			swap(heap[k], heap[r]);
			k = r;
		}		
	}
}

template <typename ElementTraits>
bool Heap<ElementTraits>::putTest(const uint8* element) const {
	return (count < countThreshold) || (ElementTraits::compare(data + heap[0], element) > 0);
}

template <typename ElementTraits>
bool Heap<ElementTraits>::put(const uint8* element) {
	// assert (*element != 0xFF)

	uint elementSize = ElementTraits::sizeOf(element);

	if (count < countThreshold) {
		if (dataSize + elementSize > dataCapacity) {
			if (dataSize + elementSize <= dataCapacity + garbageSize) {
				deflate();
			} else {
				return false;
			}
		}
		memmove(data + dataSize, element, elementSize);
		heap[count++] = dataSize;
		dataSize += elementSize;

		fixUp();

		return true;
	} 
	
	if (ElementTraits::compare(data + heap[0], element) <= 0) {
		return true;
	}

	uint size = ElementTraits::sizeOf(data + heap[0]);
	if (size < elementSize) {
		if (dataSize + elementSize > dataCapacity || garbageSize * 2 > dataSize) {
			if (dataSize + elementSize > dataCapacity + garbageSize + size) {
				return false;
			} else {
				count--;
				memset(data + heap[0], 0xFF, size);
				
				deflate();

				memmove(data + dataSize, element, elementSize);
				heap[count++] = dataSize;
				dataSize += elementSize;

				fixUp();

				return true;
			}
		}

		if (heap[0] + size < dataSize) {
			memset(data + heap[0], 0xFF, size);
			garbageSize += size;
		} else {
			dataSize -= size;
		}

		heap[0] = dataSize;
		dataSize += elementSize;
	} else if (heap[0] + size == dataSize) {
		dataSize -= size - elementSize;
	} else {
		garbageSize += size - elementSize;
		memset(data + heap[0] + elementSize, 0xFF, size - elementSize);
	}

	memmove(data + heap[0], element, elementSize);

	fixDown();

	return true;
}
